#include "Chassis.h"
#include "FastMathFunctions.h"

/* 底盘初始化 */
inline void Chassis_Init(void)
{
    AttachInterrupt_CAN(&hcan1, RMDS_CAN_Callback); // 连接迈克逊电机CAN中断回调

    /* 码盘初始化 */
    Action_Init();

    /* 初始化RMDS电机驱动器 */
    RMDS_Init(&chassis.motor1, 1);
    RMDS_Init(&chassis.motor2, 2);
    RMDS_Init(&chassis.motor3, 3);
    RMDS_ResetAll(); // 没有设置模式就会一直叫
    HAL_Delay(500);
    RMDS_SetModeAll(SPEED);

    /* 底盘x坐标pid初始化 */
    TDPID_Init(&chassis.PID_X, 5.0, 0.08f, 0.9f, 5000.0f, 3.0f, 0.005f, 500.0f);
    /* 底盘y坐标pid初始化 */
    TDPID_Init(&chassis.PID_Y, 5.0, 0.08f, 0.9f, 5000.0f, 3.0f, 0.005f, 500.0f);
    /* 底盘角度pid初始化 */
    TDPID_Init(&chassis.PID_Angle, 2.0, 0.01f, 3.0f, 2.0f, 0.1f, 0.005f, 10.0f);
}

/**
 * 底盘运行
 * 需要循环运行
 */
inline void Chassis_Run(void)
{
    if (chassis.ctrlMode == VELOCITY_MODE)
    {
        /* 底盘电机速度计算 */
        chassis.Motor1_Speed = (-chassis.setVx * fast_cos(-Action.yaw) + chassis.setVy * fast_sin(-Action.yaw) + chassis.setVw * RR) * 60 / (PI * RADIUS);
        chassis.Motor2_Speed = (chassis.setVx * fast_cos(PI / 3 - Action.yaw) - chassis.setVy * fast_sin(PI / 3 - Action.yaw) + chassis.setVw * RR) * 60 / (PI * RADIUS);
        chassis.Motor3_Speed = (chassis.setVx * fast_cos(PI / 3 + Action.yaw) + chassis.setVy * fast_sin(PI / 3 + Action.yaw) + chassis.setVw * RR) * 60 / (PI * RADIUS);
    }
    else if (chassis.ctrlMode == POSITION_MODE)
    {
        /* 位置PID计算x轴速度, y轴速度, 自转角速度 */
        TDPID_Calc(&chassis.PID_X, Action.pos_x, chassis.setPx);
        TDPID_Calc(&chassis.PID_Y, Action.pos_y, chassis.setPy);
        TDPID_Calc(&chassis.PID_Angle, Action.yaw, chassis.setYaw);
        /* 底盘速度计算 */
        chassis.Motor1_Speed = (-chassis.PID_X.out * fast_cos(-Action.yaw) + chassis.PID_Y.out * fast_sin(-Action.yaw) + chassis.PID_Angle.out * RR) * 60 / (PI * RADIUS);
        chassis.Motor2_Speed = (chassis.PID_X.out * fast_cos(PI / 3 - Action.yaw) - chassis.PID_Y.out * fast_sin(PI / 3 - Action.yaw) + chassis.PID_Angle.out * RR) * 60 / (PI * RADIUS);
        chassis.Motor3_Speed = (chassis.PID_X.out * fast_cos(PI / 3 + Action.yaw) + chassis.PID_Y.out * fast_sin(PI / 3 + Action.yaw) + chassis.PID_Angle.out * RR) * 60 / (PI * RADIUS);
    }

    /* 设置电机速度 */
    RMDS_SetSpeed(&chassis.motor1, chassis.Motor1_Speed);
    RMDS_SetSpeed(&chassis.motor2, chassis.Motor2_Speed);
    RMDS_SetSpeed(&chassis.motor3, chassis.Motor3_Speed);
}

/**
 * 设置底盘速度
 * 单位:mm/s
 * 单位:rad/s
 */
inline void Chassis_SetVel(float Vx, float Vy, float Vw)
{
    chassis.ctrlMode = VELOCITY_MODE;
    chassis.setVx = Vx;
    chassis.setVy = Vy;
    chassis.setVw = Vw;
}

/**
 * 设置底盘位置
 * 单位:mm
 * 单位:rad
 */
inline void Chassis_SetPos(float Px, float Py, float Yaw)
{
    chassis.ctrlMode = POSITION_MODE;
    chassis.setPx = Px;
    chassis.setPy = Py;
    chassis.setYaw = Yaw;
}

/**
 * 底盘到达位置
 * Px: x坐标
 * Py: y坐标
 * d : 允许偏差
 * 返回1表示到达
 */
uint8_t Chassis_Arrival(float Px, float Py, float d)
{
    /* 偏差圆 */
    float bias = (Px - Action.pos_x) * (Px - Action.pos_x) + (Py - Action.pos_y) * (Py - Action.pos_y);
    if (bias < d * d)
    {
        return 1;
    }
    else
    {
        return 0;
    }
}
